#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil
import re
import serial
import numpy as np
import cv2
import threading
import RPi.GPIO as GPIO

# 连接的pixhawk飞控的端口
connection_string = '/dev/ttyACM0'
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True, baud=921600)
cap = cv2.VideoCapture(0)  # 开启摄像头
cap.set(cv2.CAP_PROP_FRAME_WIDTH,640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,480)
cap.set(cv2.CAP_PROP_FPS,30)
GPIO.setmode(GPIO.BOARD) #引脚编码方式
GPIO.setwarnings(False) #忽略GPIO警告
FORWARD_CLEAR = 12
GPIO.setup(FORWARD_CLEAR, GPIO.IN)


#定义无人机下的坐标系
def send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration=0):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame Needs to be MAV_FRAME_BODY_NED for forward/back left/right control.
        0b0000111111000111,  # type_mask
        0, 0, 0,  # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z,  # m/s
        0, 0, 0,  # x, y, z acceleration
        0, 0)
    for x in range(0, duration):
        vehicle.send_mavlink(msg)
        time.sleep(1)


# 定义arm_and_takeoff函数，使无人机解锁并起飞到目标高度
def arm_and_takeoff(aTargetAltitude):
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True
    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)
    # 发送起飞指令
    print("Taking off!")
    vehicle.simple_takeoff(1)
    time.sleep(3)
    # 在无人机上升到目标高度之前，阻塞程序
    while True:
        print("Rangefinder distance:", vehicle.rangefinder.distance)
        if vehicle.rangefinder.distance >= 1 * 0.95:
            print("Reached target altitude")
            time.sleep(2)
            break
        else:
            send_body_ned_velocity(0, 0, -0.1, 1)
            print("UP 0.1m")
            time.sleep(1)

#根据人脸位置返回运动逻辑
def track():
    max_w = 0
    faceRects = classifier.detectMultiScale(gray,scaleFactor=1.2,minNeighbors=3,minSize=(60, 60))
    if len(faceRects):
        for faceRect in faceRects:
            x,y,w,h = faceRect
            if w > max_w:
                max_w = w
                max_x = x
                max_y = y
                max_h = h     
        cv2.rectangle(faceImg,(max_x, max_y), (max_x + max_w, max_y + max_h), (0,255,0), 2)
        face_pos = x+w/2-width/2
        print((x+w/2)-width/2)
        #----------更改此处的数值改变人脸靠太近的阈值引发的后退效果-------------
        #----------（改大：人脸贴得更近无人机才开始后退）-------------
        #----------（改小：人脸与相机更远的时候无人机也会后退）-------
        if w*h > 8000:
            print("Back")
            return 2,face_pos
        #----------更改此处的值决定无人机在检测人脸偏移屏幕中央的灵敏度-------
        #------------改大：不灵敏|改小：灵敏--------------
        if face_pos < -70 :
            print("Left")
            return -1,face_pos
        if face_pos > 70 :
            print("Right")
            return 1,face_pos
        else:
            print("Middle")
            return 0,face_pos
    return 0,0


#主程序
arm_and_takeoff(1)# 调用arm_and_takeoff函数，目标高度1m
mod = 0 #初始化模式为0
back=0
for i in range(0,40):
    ok, faceImg = cap.read()  # 读取摄像头图像
    if ok is False:
        print('无法读取到摄像头！')
        break
    high=faceImg.shape[0]
    width=faceImg.shape[1]
    gray = cv2.cvtColor(faceImg,cv2.COLOR_BGR2GRAY)
    #------------------更改路径可以使用不同的识别特征-----------------
    classifier = cv2.CascadeClassifier('/home/l1015/Camera/颜色识别跟踪/haarcascade_frontalface_alt.xml')
    thread = threading.Thread(target=track)
    thread.start()
    mod,fly_speed=track() #直到接受到信息为止才会执行判断
    fly_speed_dealed=fly_speed*0.001#根据远离中央的位置决定运动速度
    if (not GPIO.input(FORWARD_CLEAR)):#光电开关0.5m内有人强制后退
        send_body_ned_velocity(-0.5,0,0,1)
        time.sleep(0.5)
        continue;
    if (mod == 1):#向右飞
        send_body_ned_velocity(0,fly_speed_dealed,0,1)
    if (mod == -1):#向左飞
        send_body_ned_velocity(0,fly_speed_dealed,0,1)
    if (mod == 0):
        time.sleep(0.1)#无动作
    if (mod == 2):#后退
        send_body_ned_velocity(-0.15,0,0,1)


cap.release()
cv2.destroyAllWindows()
# 发送"降落"指令
print("Land")
# 降落，只需将无人机的飞行模式切换成"Land"
vehicle.mode = VehicleMode("LAND")
# 退出之前，清除vehicle对象
print("Close vehicle object")
vehicle.close()
GPIO.cleanup()



